#include "ref.h"

void ref_fir_interpolate_f32(const riscv_fir_interpolate_instance_f32 *S,
                             float32_t *pSrc, float32_t *pDst,
                             uint32_t blockSize)
{
    float32_t *pState = S->pState;         /* State pointer */
    const float32_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
    float32_t *pStateCurnt; /* Points to the current sample of the state */
    float32_t *ptr1;        /* Temporary pointer for state buffer */
    const float32_t *ptr2;  /* Temporary pointer for coefficient buffer */
    float32_t sum;          /* Accumulator */
    uint32_t i, blkCnt;     /* Loop counters */
    uint16_t phaseLen = S->phaseLength,
             tapCnt; /* Length of each polyphase filter component */

    /* S->pState buffer contains previous frame (phaseLen - 1) samples */
    /* pStateCurnt points to the location where the new input data should be
     * written */
    pStateCurnt = S->pState + phaseLen - 1;

    /* Total number of intput samples */
    blkCnt = blockSize;

    /* Loop over the blockSize. */
    while (blkCnt > 0U) {
        /* Copy new input sample into the state buffer */
        *pStateCurnt++ = *pSrc++;

        /* Loop over the Interpolation factor. */
        i = S->L;

        while (i > 0U) {
            /* Set accumulator to zero */
            sum = 0.0f;

            /* Initialize state pointer */
            ptr1 = pState;

            /* Initialize coefficient pointer */
            ptr2 = pCoeffs + i - 1;

            /* Loop over the polyPhase length */
            tapCnt = phaseLen;

            while (tapCnt > 0U) {
                /* Perform the multiply-accumulate */
                sum += *ptr1++ * *ptr2;

                /* Increment the coefficient pointer by interpolation factor
                 * times. */
                ptr2 += S->L;

                /* Decrement the loop counter */
                tapCnt--;
            }

            /* The result is in the accumulator, store in the destination
             * buffer. */
            *pDst++ = sum;

            /* Decrement the loop counter */
            i--;
        }

        /* Advance the state pointer by 1
         * to process the next group of interpolation factor number samples */
        pState = pState + 1;

        /* Decrement the loop counter */
        blkCnt--;
    }

    /* Processing is complete.
     ** Now copy the last phaseLen - 1 samples to the start of the state buffer.
     ** This prepares the state buffer for the next function call. */

    /* Points to the start of the state buffer */
    pStateCurnt = S->pState;

    tapCnt = phaseLen - 1U;

    while (tapCnt > 0U) {
        *pStateCurnt++ = *pState++;

        /* Decrement the loop counter */
        tapCnt--;
    }
}

void ref_fir_interpolate_q31(const riscv_fir_interpolate_instance_q31 *S,
                             q31_t *pSrc, q31_t *pDst, uint32_t blockSize)
{
    q31_t *pState = S->pState;         /* State pointer */
    const q31_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
    q31_t *pStateCurnt; /* Points to the current sample of the state */
    q31_t *ptr1;        /* Temporary pointer for state buffer */
    const q31_t *ptr2;  /* Temporary pointer for coefficient buffer */
    q63_t sum;          /* Accumulator */
    q31_t x0, c0; /* Temporary variables to hold state and coefficient values */
    uint32_t i, blkCnt; /* Loop counters */
    uint16_t phaseLen = S->phaseLength,
             tapCnt; /* Length of each polyphase filter component */

    /* S->pState buffer contains previous frame (phaseLen - 1) samples */
    /* pStateCurnt points to the location where the new input data should be
     * written */
    pStateCurnt = S->pState + (q31_t)phaseLen - 1;

    /* Total number of intput samples */
    blkCnt = blockSize;

    /* Loop over the blockSize. */
    while (blkCnt > 0U) {
        /* Copy new input sample into the state buffer */
        *pStateCurnt++ = *pSrc++;

        /* Loop over the Interpolation factor. */
        i = S->L;

        while (i > 0U) {
            /* Set accumulator to zero */
            sum = 0;

            /* Initialize state pointer */
            ptr1 = pState;

            /* Initialize coefficient pointer */
            ptr2 = pCoeffs + i - 1;

            tapCnt = phaseLen;

            while (tapCnt > 0U) {
                /* Read the coefficient */
                c0 = *(ptr2);

                /* Increment the coefficient pointer by interpolation factor
                 * times. */
                ptr2 += S->L;

                /* Read the input sample */
                x0 = *ptr1++;

                /* Perform the multiply-accumulate */
                sum += (q63_t)x0 * c0;

                /* Decrement the loop counter */
                tapCnt--;
            }

            /* The result is in the accumulator, store in the destination
             * buffer. */
            *pDst++ = (q31_t)(sum >> 31);

            /* Decrement the loop counter */
            i--;
        }

        /* Advance the state pointer by 1
         * to process the next group of interpolation factor number samples */
        pState = pState + 1;

        /* Decrement the loop counter */
        blkCnt--;
    }

    /* Processing is complete.
     ** Now copy the last phaseLen - 1 samples to the satrt of the state buffer.
     ** This prepares the state buffer for the next function call. */

    /* Points to the start of the state buffer */
    pStateCurnt = S->pState;

    tapCnt = phaseLen - 1U;

    /* copy data */
    while (tapCnt > 0U) {
        *pStateCurnt++ = *pState++;

        /* Decrement the loop counter */
        tapCnt--;
    }
}

void ref_fir_interpolate_q15(const riscv_fir_interpolate_instance_q15 *S,
                             q15_t *pSrc, q15_t *pDst, uint32_t blockSize)
{
    q15_t *pState = S->pState;         /* State pointer         */
    const q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
    q15_t *pStateCurnt; /* Points to the current sample of the state */
    q15_t *ptr1;        /* Temporary pointer for state buffer */
    const q15_t *ptr2;  /* Temporary pointer for coefficient buffer */
    q63_t sum;          /* Accumulator */
    q15_t x0, c0; /* Temporary variables to hold state and coefficient values */
    uint32_t i, blkCnt, tapCnt; /* Loop counters */
    uint16_t phaseLen =
        S->phaseLength; /* Length of each polyphase filter component */

    /* S->pState buffer contains previous frame (phaseLen - 1) samples */
    /* pStateCurnt points to the location where the new input data should be
     * written */
    pStateCurnt = S->pState + phaseLen - 1;

    /* Total number of intput samples */
    blkCnt = blockSize;

    /* Loop over the blockSize. */
    while (blkCnt > 0U) {
        /* Copy new input sample into the state buffer */
        *pStateCurnt++ = *pSrc++;

        /* Loop over the Interpolation factor. */
        i = S->L;

        while (i > 0U) {
            /* Set accumulator to zero */
            sum = 0;

            /* Initialize state pointer */
            ptr1 = pState;

            /* Initialize coefficient pointer */
            ptr2 = pCoeffs + i - 1;

            /* Loop over the polyPhase length */
            tapCnt = (uint32_t)phaseLen;

            while (tapCnt > 0U) {
                /* Read the coefficient */
                c0 = *ptr2;

                /* Increment the coefficient pointer by interpolation factor
                 * times. */
                ptr2 += S->L;

                /* Read the input sample */
                x0 = *ptr1++;

                /* Perform the multiply-accumulate */
                sum += (q31_t)x0 * c0;

                /* Decrement the loop counter */
                tapCnt--;
            }

            /* Store the result after converting to 1.15 format in the
             * destination buffer */
            *pDst++ = ref_sat_q15(sum >> 15);

            /* Decrement the loop counter */
            i--;
        }

        /* Advance the state pointer by 1
         * to process the next group of interpolation factor number samples */
        pState = pState + 1;

        /* Decrement the loop counter */
        blkCnt--;
    }

    /* Processing is complete.
     ** Now copy the last phaseLen - 1 samples to the start of the state buffer.
     ** This prepares the state buffer for the next function call. */

    /* Points to the start of the state buffer */
    pStateCurnt = S->pState;

    i = (uint32_t)phaseLen - 1U;

    while (i > 0U) {
        *pStateCurnt++ = *pState++;

        /* Decrement the loop counter */
        i--;
    }
}
